import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
import cv2
import numpy as np

class MapPublisher(Node):

    def __init__(self):
        super().__init__('map_publisher')
        self.declare_parameter('map_path','my_map.pgm')
        self.publisher_ = self.create_publisher(OccupancyGrid, 'map', 10)

    def publish_map(self):
        map_path=self.get_parameter('map_path').get_parameter_value().string_value
        map_image = cv2.imread(map_path, cv2.IMREAD_UNCHANGED)
        # 对数组（例如图像）中的每个元素进行按位求反操作
        map_image = np.invert(map_image)  # Invert the image if negate is set to 1
        # 镜像反转，因为图像坐标系的原点在左上，地图在左下
        map_image=cv2.flip(map_image, 0)
        occupancy_grid = OccupancyGrid()
        occupancy_grid.header.frame_id = 'map'
        occupancy_grid.info.resolution = 0.05
        occupancy_grid.info.width = map_image.shape[1]
        occupancy_grid.info.height = map_image.shape[0]
        occupancy_grid.info.origin.position.x = -10.0
        occupancy_grid.info.origin.position.y = -10.0
        occupancy_grid.info.origin.position.z = 0.0
        map_data = map_image.flatten()  # Flatten the map image array
        element_types = np.unique(map_data)
        print(element_types)
        map_data[map_data == 255] = 100
        map_data[map_data == 1] = 0
        # 这边看是-1，实际上值为255
        map_data[map_data == 50] = -1
        occupancy_grid.data = map_data.astype(np.int8).tolist()  # Convert the data to int8
        print(map_data)
        self.publisher_.publish(occupancy_grid)

def main(args=None):
    rclpy.init(args=args)
    map_publisher = MapPublisher()
    map_publisher.publish_map()
    rclpy.spin_once(map_publisher)
    map_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
